/*
 * Copyright (c) 2012, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Humanoid Robotics Lab      Georgia Institute of Technology
 * Director: Mike Stilman     http://www.golems.org
 */

#include "tabRipPlanner.h"

#include <wx/wx.h>
#include <GUI/Viewer.h>
#include <GUI/GUI.h>
#include <GUI/GRIPSlider.h>
#include <GUI/GRIPFrame.h>
#include <Tabs/GRIPTab.h>
#include <robotics/Robot.h>

#include <iostream>
#include <algorithm>
#include <iterator>

#include <Tabs/AllTabs.h>
#include <GRIPApp.h>

#define MAX_NODES	100000
#define STEPS_BACK	5


/* Quick intro to adding tabs:
 * 1- Copy template cpp and header files and replace with new class name
 * 2- include classname.h in AllTabs.h, and use the ADD_TAB macro to create it
 */

// Control IDs (used for event handling - be sure to start with a non-conflicted id)
enum planTabEvents {
	button_SetStart = 50,
	button_SetGoal,
	button_showStart,
	button_showGoal,
	button_resetPlanner,
	button_empty1,
	button_empty2,
	button_Plan,
	button_Stop,
	button_UpdateTime,
	button_UpdateGreedyRatio,
	button_UpdateRobotId,
	button_ExportSequence,
	button_ShowPath,
	checkbox_beGreedy,
	checkbox_useConnect,
	checkbox_useSmooth,
	slider_Time,
};

// sizer for whole tab
wxBoxSizer* sizerFull;

//Add a handler for any events that can be generated by the widgets you add here (sliders, radio, checkbox, etc)
BEGIN_EVENT_TABLE(RipPlannerTab, wxPanel)
EVT_COMMAND (wxID_ANY, wxEVT_GRIP_SLIDER_CHANGE, RipPlannerTab::OnSlider)
EVT_COMMAND (wxID_ANY, wxEVT_COMMAND_RADIOBOX_SELECTED, RipPlannerTab::OnRadio)
EVT_COMMAND (wxID_ANY, wxEVT_COMMAND_BUTTON_CLICKED, RipPlannerTab::OnButton)
EVT_COMMAND (wxID_ANY, wxEVT_COMMAND_CHECKBOX_CLICKED, RipPlannerTab::OnCheckBox)
END_EVENT_TABLE()

// Class constructor for the tab: Each tab will be a subclass of RSTTab
IMPLEMENT_DYNAMIC_CLASS(RipPlannerTab, GRIPTab)

/**
 * @function RipTabPlanner
 * @brief Constructor
 */
RipPlannerTab::RipPlannerTab( wxWindow *parent, const wxWindowID id,
		              const wxPoint& pos, const wxSize& size, long style) :
	                      GRIPTab(parent, id, pos, size, style) {

    mStartConf.resize(0);
    mGoalConf.resize(0);
    mLinks.resize(0);

	robotId = -1;
    mRrtStyle = 0;
    mGreedyMode = false;
    mConnectMode = false;
    mSmooth = false;
    mPlanner = NULL;

    sizerFull = new wxBoxSizer( wxHORIZONTAL );
 
    // ** Create left static box for configuring the planner **

    // Create StaticBox container for all items
    wxStaticBox* configureBox = new wxStaticBox(this, -1, wxT("Configure"));

    // Create sizer for this box with horizontal layout
    wxStaticBoxSizer* configureBoxSizer = new wxStaticBoxSizer(configureBox, wxHORIZONTAL);

    // Create a sizer for radio buttons in 1st column
    wxBoxSizer *col1Sizer = new wxBoxSizer(wxVERTICAL);
    wxBoxSizer *miniSizer = new wxBoxSizer(wxVERTICAL); // annoying hack to get checkboxes close together
    miniSizer->Add( new wxCheckBox(this, checkbox_beGreedy, _T("&goal bias (be greedy)")),
		    1, // vertical stretch evenly
		    wxALIGN_NOT,
		    0);
    miniSizer->Add( new wxCheckBox(this, checkbox_useConnect, _T("use &connect algorithm (be really greedy)")),
		    1, // vertical stretch evenly
		    wxALIGN_NOT,
		    0 );
    miniSizer->Add( new wxCheckBox(this, checkbox_useSmooth, _T("use &smoother (make it less ugly)")),
		    1, // vertical stretch evenly
		    wxALIGN_NOT,
		    0 );
    col1Sizer->Add(miniSizer,1,wxALIGN_NOT,0);

    // Create radio button for rrt_style
    static const wxString RRTStyles[] =
    {
        wxT("Single"),
	wxT("Bi-directional")
    };
    col1Sizer->Add( new wxRadioBox(this, wxID_ANY, wxT("RRT &style:"),
		    wxDefaultPosition, wxDefaultSize, WXSIZEOF(RRTStyles), RRTStyles, 1,
		    wxRA_SPECIFY_ROWS),
		    1, // stretch evenly with buttons and checkboxes
		    wxALIGN_NOT,
		    0 );
    // Add col1 to configureBoxSizer
    configureBoxSizer->Add( col1Sizer,
			    3, // 3/5 of configure box
			    wxALIGN_NOT,
			    0 ); //

    // Create sizer for start buttons in 2nd column
    wxBoxSizer *col2Sizer = new wxBoxSizer(wxVERTICAL);
    col2Sizer->Add( new wxButton(this, button_SetStart, wxT("Set &Start")),
		    0, // make horizontally unstretchable
		    wxALL, // make border all around (implicit top alignment)
		    1 ); // set border width to 1, so start buttons are close together
    col2Sizer->Add( new wxButton(this, button_showStart, wxT("Show S&tart")),
		    0, // make horizontally unstretchable
		    wxALL, // make border all around (implicit top alignment)
		    1 ); // set border width to 1, so start buttons are close together
    col2Sizer->Add( new wxButton(this, button_empty1, wxT("Check collision")),
		    0, // make horizontally unstretchable
		    wxALL, // make border all around (implicit top alignment)
		    1 ); // set border width to 1, so start buttons are close together


    // Add col2Sizer to the configuration box
    configureBoxSizer->Add( col2Sizer,
			    1, // takes half the space of the configure box
			    wxALIGN_NOT ); // no border and center horizontally

    // Create sizer for goal buttons in 3rd column
    wxBoxSizer *col3Sizer = new wxBoxSizer(wxVERTICAL);
    col3Sizer->Add( new wxButton(this, button_SetGoal, wxT("Set &Goal")),
		    0, // make horizontally unstretchable
		    wxALL, // make border all around (implicit top alignment)
		    1 ); // set border width to 1, so start buttons are close together
    col3Sizer->Add( new wxButton(this, button_showGoal, wxT("Show G&oal")),
		    0, // make horizontally unstretchable
		    wxALL, // make border all around (implicit top alignment)
		    1 ); // set border width to 1, so start buttons are close together
    col3Sizer->Add( new wxButton(this, button_empty2, wxT("Plan Separate")),
		    0, // make horizontally unstretchable
		    wxALL, // make border all around (implicit top alignment)
		    1 ); // set border width to 1, so start buttons are close together
    configureBoxSizer->Add( col3Sizer,
			    1, // size evenly with radio box and checkboxes
			    wxALIGN_NOT ); // no border and center horizontally

    // Add this box to parent sizer
    sizerFull->Add( configureBoxSizer,
		    4, // 4-to-1 ratio with execute sizer, since it just has 3 buttons
		    wxEXPAND | wxALL,
		    6 );


    // ** Create right static box for running the planner **
    wxStaticBox* executeBox = new wxStaticBox(this, -1, wxT("Execute Planner"));

    // Create sizer for this box
    wxStaticBoxSizer* executeBoxSizer = new wxStaticBoxSizer(executeBox, wxVERTICAL);

    // Add buttons for "plan", "save movie", and "show path"
    executeBoxSizer->Add( new wxButton(this, button_Plan, wxT("&Start")),
	 		  1, // stretch to fit horizontally
			  wxGROW ); // let it hog all the space in it's column

    executeBoxSizer->Add( new wxButton(this, button_Stop, wxT("&Stop")),
			  1, // stretch to fit horizontally
			  wxGROW );


    wxBoxSizer *timeSizer = new wxBoxSizer(wxHORIZONTAL);
    mTimeText = new wxTextCtrl(this,1008,wxT("5.0"),wxDefaultPosition,wxSize(40,20),wxTE_RIGHT);//,wxTE_PROCESS_ENTER | wxTE_RIGHT);
    timeSizer->Add( mTimeText,2,wxALL,1 );
    timeSizer->Add(new wxButton(this, button_UpdateTime, wxT("Set T(s)")),2,wxALL,1);
    executeBoxSizer->Add(timeSizer,1,wxALL,2);

    executeBoxSizer->Add( new wxButton(this, button_ShowPath, wxT("&Print")),
			  1, // stretch to fit horizontally
			  wxGROW );

    sizerFull->Add(executeBoxSizer, 1, wxEXPAND | wxALL, 6);

    SetSizer(sizerFull);


	wxBoxSizer *greedySizer = new wxBoxSizer(wxHORIZONTAL);
	mGreedyText = new wxTextCtrl(this, 1008, wxT("0"), wxDefaultPosition, wxSize(40,20), wxTE_RIGHT);
	greedySizer->Add(mGreedyText,2,wxALL,1);
	greedySizer->Add(new wxButton(this, button_UpdateGreedyRatio, wxT("Set G-Ratio")),2,wxALL,1);
	col1Sizer->Add(greedySizer,1,wxALL,2);

	wxBoxSizer *robotSizer = new wxBoxSizer(wxHORIZONTAL);
	mRobotText = new wxTextCtrl(this, 1008, wxT("-1"), wxDefaultPosition, wxSize(40,20), wxTE_RIGHT);
	robotSizer->Add(mRobotText,2,wxALL,1);
	robotSizer->Add(new wxButton(this, button_UpdateRobotId, wxT("Set Robot")),2,wxALL,1);
	col3Sizer->Add(robotSizer,1,wxALL,2);

}

/**
 * @function OnRadio
 * @brief Handle Radio toggle
 */
void RipPlannerTab::OnRadio(wxCommandEvent &evt) {

	mRrtStyle = evt.GetSelection();
	std::cout << "rrtStyle = " << mRrtStyle << std::endl;
}

/**
 * @function OnButton
 * @brief Handle Button Events
 */
void RipPlannerTab::OnButton(wxCommandEvent &evt) {
  
  int button_num = evt.GetId();
  
  switch (button_num) {
    
    /** Set Start */
  case button_SetStart:
    if ( mWorld != NULL ) {
      if( mWorld->getNumRobots() < 1) {
	std::cout << "(!) Must have a world with a robot to set a Start state" << std::endl;
	break;
      }

		if (robotId < 0) { // multi robot
			int size = 0;
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				size += mWorld->getRobot(i)->getQuickDofs().size();
			}
			mStartConf.resize(size);

			if (robotSubset.size() != mWorld->getNumRobots()) {
				robotSubset.clear();
				for (int i = 0; i < mWorld->getNumRobots(); i++) {
					robotSubset.push_back(i);
				}
			}

			int offset = 0;
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				robotics::Robot* robot = mWorld->getRobot(i);
				std::cout << "(i) Setting Start state for " << robot->getName() << ":" << std::endl;
			
				//mStartConf = mWorld->getRobot(i)->getQuickDofs();
				for (int j = 0; j < robot->getQuickDofs().size(); j++) {
					mStartConf(offset+j) = robot->getQuickDofs()(j);
				}
				offset += robot->getQuickDofs().size();

				// Single robot - multiple RRTs
				std::map<int,Eigen::VectorXd>::iterator it;
				it = mStartConfs.find(i);
				if (it == mStartConfs.end())
					mStartConfs.insert(std::pair<int,Eigen::VectorXd>(i,mWorld->getRobot(i)->getQuickDofs()));
				else
					it->second = mWorld->getRobot(i)->getQuickDofs();
			}
		} else {
			mStartConf.resize(mWorld->getRobot(robotId)->getQuickDofs().size());
			mStartConf = mWorld->getRobot(robotId)->getQuickDofs();
			// Single robot - multiple RRTs
			std::map<int,Eigen::VectorXd>::iterator it;
			it = mStartConfs.find(robotId);
			if (it == mStartConfs.end())
				mStartConfs.insert(std::pair<int,Eigen::VectorXd>(robotId,mWorld->getRobot(robotId)->getQuickDofs()));
			else
				it->second = mWorld->getRobot(robotId)->getQuickDofs();
		}


      for( unsigned int i = 0; i < mStartConf.size(); i++ )
	{  std::cout << mStartConf(i) << " ";  } 
      std::cout << std::endl;
    } else {
      std::cout << "(!) Must have a world loaded to set a Start state." << std::endl;
    }
    break;
    
    /** Set Goal */
  case button_SetGoal:
    if ( mWorld != NULL ) {
      if( mWorld->getNumRobots() < 1){
	std::cout << "(!) Must have a world with a robot to set a Goal state.(!)" << std::endl;
	break;
      }

		if (robotId < 0) { // multi robot
			int size = 0;
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				size += mWorld->getRobot(i)->getQuickDofs().size();
			}
			mGoalConf.resize(size);

			if (robotSubset.size() != mWorld->getNumRobots()) {
				robotSubset.clear();
				for (int i = 0; i < mWorld->getNumRobots(); i++) {
					robotSubset.push_back(i);
				}
			}

			int offset = 0;
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				robotics::Robot* robot = mWorld->getRobot(i);
				std::cout << "(i) Setting goal state for " << robot->getName() << ":" << std::endl;
			
				//mGoalConf = mWorld->getRobot(i)->getQuickDofs();
				for (int j = 0; j < robot->getQuickDofs().size(); j++) {
					mGoalConf(offset+j) = robot->getQuickDofs()(j);
				}
				offset += robot->getQuickDofs().size();

				// Single robot - multiple RRTs
				std::map<int,Eigen::VectorXd>::iterator it;
				it = mGoalConfs.find(i);
				if (it == mGoalConfs.end())
					mGoalConfs.insert(std::pair<int,Eigen::VectorXd>(i,mWorld->getRobot(i)->getQuickDofs()));
				else
					it->second = mWorld->getRobot(i)->getQuickDofs();
				}
		} else {
			//single robot
			mGoalConf.resize(mWorld->getRobot(robotId)->getQuickDofs().size());
			mGoalConf = mWorld->getRobot(robotId)->getQuickDofs();
			// Single robot - multiple RRTs
			std::map<int,Eigen::VectorXd>::iterator it;
			it = mGoalConfs.find(robotId);
			if (it == mGoalConfs.end())
				mGoalConfs.insert(std::pair<int,Eigen::VectorXd>(robotId,mWorld->getRobot(robotId)->getQuickDofs()));
			else
				it->second = mWorld->getRobot(robotId)->getQuickDofs();
		}
      
      for( unsigned int i = 0; i < mGoalConf.size(); i++ )
	{ std::cout << mGoalConf(i) << " "; } 
      std::cout << std::endl;
    } else {
      std::cout << "(!) Must have a world loaded to set a Goal state"<< std::endl;
    }
    break;
    
    /** Show Start */
  case button_showStart:
    if( mStartConf.size() < 1 ){
      std::cout << "(x) First, set a start configuration" << std::endl;
      break;
    } 
	{
		if (robotId < 0) { // multi robot
			int offset = 0;
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				robotics::Robot* robot = mWorld->getRobot(i);

				Eigen::VectorXd start;
				start.resize(robot->getQuickDofs().size());
				for (int j = offset; j < offset+start.size(); j++) {
					start(j-offset) = mStartConf(j);
				}
				offset += start.size();
				robot->setQuickDofs(start);
			}
		} else {
			// single robot
			mWorld->getRobot(robotId)->setQuickDofs(mStartConf);
		}
	}
    
    for( unsigned int i = 0; i< mStartConf.size(); i++ )
      {
		  std::cout << mStartConf(i) << " "; 
	}
    std::cout << std::endl;
    
	for (int i = 0; i < mWorld->getNumRobots(); i++) {
		mWorld->getRobot(i)->update();
	}
    viewer->UpdateCamera(); 
    break;
    
    /** Show Goal */
  case button_showGoal:
    if( mGoalConf.size() < 1 ){
      std::cout << "(x) First, set a goal configuration" << std::endl;
      break;
    }
	{
		if (robotId < 0) { // multi robot
			int offset = 0;
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				robotics::Robot* robot = mWorld->getRobot(i);

				Eigen::VectorXd goal;
				goal.resize(robot->getQuickDofs().size());
				for (int j = offset; j < offset+goal.size(); j++) {
					goal(j-offset) = mGoalConf(j);
				}
				offset += goal.size();
				robot->setQuickDofs(goal);
			}
		} else {
			// single
			mWorld->getRobot(robotId)->setQuickDofs(mGoalConf);
		}
	}
    
    for( unsigned int i = 0; i< mGoalConf.size(); i++ )
      {  std::cout << mGoalConf[i] << " ";  }
    std::cout << std::endl;
    
	for (int i = 0; i < mWorld->getNumRobots(); i++) {
		mWorld->getRobot(i)->update();
	}
    viewer->UpdateCamera(); 
    break;
    
    /** Reset Planner */ 
  case button_resetPlanner:
    if ( mWorld != NULL) {
      if ( mPlanner != NULL)
	delete mPlanner;
      
      std::cout << "Creating a new planner" << std::endl;
      double stepSize = 0.02; // default
      mPlanner = new PathPlanner( *mWorld, false, stepSize );
    } else {
      std::cout << "(!) Must have a world loaded to make a planner" << std::endl;
    }
    break;
    
    /** Empty button 1 */
  case button_empty1:
    {
      std::cout << "(0) Checking Collisions" << std::endl;
      bool st;
      st = mWorld->checkCollision();
      if( st == true )
	{ printf("Collisions \n");}
      else
	{ printf("No Collisions \n");}
      
    }
    break;
    
    /** Separate Plan Button - Execute plans individually and check collisions by merging plans */
  case button_empty2:{
	init = clock();
    std::cout << "-- (0) Planning individually all robots (0)--" << std::endl;
      
      double stepSize = 0.05;
      bool result = true;
	Eigen::VectorXi tempLinks;

		if( mWorld == NULL ){ std::cout << "(x) Must load a world" << std::endl; result = false;break; }
      	if( mWorld->getNumRobots() < 1){ std::cout << "(x) Must load a world with a robot" << std::endl; result = false;break; }

	for (int i = 0; i < mWorld->getNumRobots(); i++) {
		if( mGoalConfs.find(i) == mGoalConfs.end() ){ std::cout << "(x) Must set a goal for Robot #" << i << std::endl; result = false;break; }
  		if( mStartConfs.find(i) == mStartConfs.end() ){ std::cout << "(x) Must set a start for Robot #" << i  << std::endl; result = false;break; }
      	
		
		// reset to start
		int offset = 0;
		for (int k = 0; k < mWorld->getNumRobots(); k++) {
			robotics::Robot* robot = mWorld->getRobot(k);

			Eigen::VectorXd start;
			start.resize(robot->getQuickDofs().size());
			for (int j = offset; j < offset+start.size(); j++) {
				start(j-offset) = mStartConf(j);
			}
			offset += start.size();
			robot->setQuickDofs(start);
			robot->update();
		}
	

		PathPlanner* planner0 = new PathPlanner(*mWorld,false,stepSize);
		tempLinks = mWorld->getRobot(i)->getQuickDofsIndices();


		std::cout << "-- (0) Planning individually Robot " << mWorld->getRobot(i)->getName() << std::endl;

    		result = result && planner0->planPath(
					i,
					tempLinks, 
					mStartConfs[i], 
					mGoalConfs[i], 
					robotSubset,
					mRrtStyle,  
					mConnectMode,
					mGreedyMode,
					mGreedyRatio,
					mSmooth, 
					MAX_NODES);

		
		// Creates or Updates the path 
			std::map<int,std::list<Eigen::VectorXd> >::iterator it;
			it = paths.find(i);
			if (it == paths.end()){
				paths.insert(std::pair<int,std::list<Eigen::VectorXd> >(i,planner0->path));
				std::cout << "Creating path entry for Robot " << mWorld->getRobot(i)->getName()	<< std::endl;		
			}			
			else{
				it->second = planner0->path;
				std::cout << "Updating path entry for Robot " << mWorld->getRobot(i)->getName()	<< std::endl;				
			}	

	}
	
	if (result){


		// Concatenates plans
		std::cout << "Mergin plans in High-dimension space" << std::endl;

		// New empty plan
		std::list<Eigen::VectorXd> HDPlan;

		// Gets the size of the space 
		int size = 0;
		bool done = false;
		for (int i = 0; i < mWorld->getNumRobots(); i++) {
				size += mWorld->getRobot(i)->getQuickDofs().size();
		}
		
		int path_pos = 0;
		
		while (!done){

			done = true;
			int offset = 0;
			Eigen::VectorXd pose;
			pose.resize(size);
			for (int i = 0; i < mWorld->getNumRobots(); i++) {

				std::list<Eigen::VectorXd>::iterator it = paths[i].begin();

				//it = it + path_pos;

					for (int m = 0; m<path_pos; m++)
					it++;
				

				bool armDone = (path_pos>=paths[i].size());

				if (armDone){
					it = paths[i].end();
					it--;
				} 
// Repeats last position for arms with shorter paths, doesnt work if plan empty, fix that
				
				done &= armDone;

				int armSize = mWorld->getRobot(i)->getQuickDofs().size();

				for (int j=0; j< armSize;j++){ // This loop fills the HD plan, link by link for arm i
					pose[offset+j] = (*it)[j];
				}

				offset += armSize;



			}// End for i (all arms considered)
			//Adds the pose to the plan
			HDPlan.push_back(pose);

		path_pos++;


		}// End while (Plan concatenation done)


	//mPlanner->path = HDLine;

	// calc mLinks
	size = 0;
	for (int i = 0; i < mWorld->getNumRobots(); i++) {
		size += mWorld->getRobot(i)->getQuickDofs().size();
	}
	mLinks.resize(size);
	int offset = 0;
	for (int i = 0; i < mWorld->getNumRobots(); i++) {
		robotics::Robot* robot = mWorld->getRobot(i);
		for (int j = 0; j < robot->getQuickDofsIndices().size(); j++) {
			mLinks(j+offset) = robot->getQuickDofsIndices()(j);
		}
		offset += robot->getQuickDofsIndices().size();
	}

	std::cout << "HDPlan size = " << HDPlan.size() << "\n";

	// check for collisions in HDPlan
	std::vector<bool> collisions;
	for (std::list<Eigen::VectorXd>::iterator i = HDPlan.begin(); i != HDPlan.end(); i++) {
		collisions.push_back(checkCollision(*i, mLinks));
	}

	//now plan detours around collisions
	int j = 1;
	std::list<Eigen::VectorXd>::iterator prevI = HDPlan.begin();
	std::list<Eigen::VectorXd>::iterator i = prevI;
	for (i++; i != HDPlan.end();) {
		if (collisions[j]) {// collision here, need to remove and plan around to next open spot
			std::cout << "detouring around " << j << "\n";
			// find next opening
			int nextJ = j+1;
			std::list<Eigen::VectorXd>::iterator nextI = i;
			int count = 0;
			for (nextI++; collisions[nextJ] && nextJ < collisions.size(); nextJ++, nextI++) {
				/*if (!collisions[nextJ] && ++count >= STEPS_BACK) {
					break;
				} else if (collisions[nextJ]) {
					count = 0;
				}*/	
			}
			/*if (nextJ == collisions.size()) {
				do  {
					nextI--;
				} while (collisions[--nextJ]);
			}
			for (int k = j-1; k > 0 && k > j-1-STEPS_BACK; k--) {
				prevI--;
			}*/
			std::cout << "next location is: " << nextJ << "\n";

			// set robot config to just before collision
			int config_offset = 0;
			for (int k = 0; k < mWorld->getNumRobots(); k++) {
				robotics::Robot* robot = mWorld->getRobot(k);

				Eigen::VectorXd goal;
				goal.resize(robot->getQuickDofs().size());
				for (int j = config_offset; j < config_offset+goal.size(); j++) {
					goal(j-config_offset) = (*prevI)(j);
				}
				config_offset += goal.size();
				robot->setQuickDofs(goal);
				robot->update();
			}

			// figure out which robots are colliding
			collidingRobots(prevI, nextI);

			// calc mLinks
			int link_size = 0;
			for (int k = 0; k < robotSubset.size(); k++) {
				link_size += mWorld->getRobot(robotSubset[k])->getQuickDofs().size();
			}
			Eigen::VectorXi links;
			links.resize(link_size);
			config_offset = 0;
			for (int k = 0; k < robotSubset.size(); k++) {
				robotics::Robot* robot = mWorld->getRobot(robotSubset[k]);
				for (int l = 0; l < robot->getQuickDofsIndices().size(); l++) {
					links(l+config_offset) = robot->getQuickDofsIndices()(l);
				}
				config_offset += robot->getQuickDofsIndices().size();
			}

			Eigen::VectorXd start;
			Eigen::VectorXd goal;
			start.resize(link_size);
			goal.resize(link_size);
			int r = 0;
			config_offset = 0;
			int subset_offset = 0;
			for (int k = 0; k < mWorld->getNumRobots(); k++) {
				robotics::Robot* robot = mWorld->getRobot(k);
				int dofs = robot->getQuickDofs().size();
				if (r < robotSubset.size() && k == robotSubset[r]) {
					// this robot is part of it
					for (int j = subset_offset; j < subset_offset+dofs; j++) {
						start(j) = (*prevI)(j-subset_offset+config_offset);
						goal(j) = (*nextI)(j-subset_offset+config_offset);
					}
					subset_offset += dofs;
					r++;
				}
				config_offset += dofs;
			}

			std::cout << start.size() << " x " << goal.size() << " x " << links.size() << "\n";

			config_offset = 0;
			for (int k = 0; k < mWorld->getNumRobots(); k++) {
				robotics::Robot* robot = mWorld->getRobot(k);

				Eigen::VectorXd goal;
				goal.resize(robot->getQuickDofs().size());
				for (int j = config_offset; j < config_offset+goal.size(); j++) {
					goal(j-config_offset) = (*prevI)(j);
				}
				config_offset += goal.size();
				robot->setQuickDofs(goal);
				robot->update();
			}

			mPlanner = new PathPlanner(*mWorld,false,stepSize);
			// plan from prevI to nextI
			result = mPlanner->planPath(
					-1, // all robots
					links,
					start,
					goal,
					robotSubset,
					mRrtStyle,  
					mConnectMode,
					mGreedyMode,
					mGreedyRatio,
					mSmooth, 
					MAX_NODES);
			// repeat config of *prevI for all non-colliding robots, smoother will handle the stall

			if (!result) { // crap, screwed up
				std::cout << "could not get back onto roadmap\n";
				break;
			}

			// insert new plan starting after prevI (before i)
			for (std::list<Eigen::VectorXd>::iterator k = mPlanner->path.begin(); k != mPlanner->path.end(); k++) {
				Eigen::VectorXd fullK;
				// expand mPlanner->path to proper size
				fullK.resize(mStartConf.size());
				r = 0;
				config_offset = 0;
				subset_offset = 0;
				for (int l = 0; l < mWorld->getNumRobots(); l++) {
					robotics::Robot* robot = mWorld->getRobot(l);
					int dofs = robot->getQuickDofs().size();
					if (r < robotSubset.size() && l == robotSubset[r]) {
						// place to put plan
						for (int n = subset_offset; n < subset_offset+dofs; n++) {
							fullK(n-subset_offset+config_offset) = (*k)(n);
						}
						subset_offset += dofs;
						r++;
					} else {
						// repeat the other robot's config
						for (int n = config_offset; n < config_offset+dofs; n++) {
							fullK(n) = (*prevI)(n);
						}
					}
					config_offset += dofs;
				}

				HDPlan.insert(i, fullK);
				prevI++;
			}

			// remove steps i to just before nextI (by setting changed arms to *prevI--stall)
			j = nextJ+1;
			for (std::list<Eigen::VectorXd>::iterator k = i; k != nextI; k++) {
				r = 0;
				config_offset = 0;
				for (int l = 0; l < mWorld->getNumRobots(); l++) {
					robotics::Robot* robot = mWorld->getRobot(l);
					int dofs = robot->getQuickDofs().size();
					if (r < robotSubset.size() && l == robotSubset[r]) {
						for (int n = config_offset; n < config_offset+dofs; n++) {
							(*k)(n) = (*prevI)(n);
						}
						r++;
					} else if (r >= robotSubset.size()) {
						break;
					}
					config_offset += dofs;
				}
			}
			
			prevI = nextI;
			i = prevI;
			i++;
			continue;
		}
		i++, j++, prevI++;
	}

	if (!result) {
		break;
	}

	mPlanner = new PathPlanner(*mWorld, false, stepSize);
	//mPlanner->smoothPath(-1, mLinks, HDPlan, robotSubset);

	SetTimeline(HDPlan);

	}// End if result (All plans worked)
	else{

		std::cout << "At least one of the plans could not be calculated - Process canceled" << std::endl;
	}




}
	std::cout << "\n\nTime for Separated plan: " << (double)(clock()-init)/((double)CLOCKS_PER_SEC) << std::endl;
    break;
    
    /** Execute Plan */
  case button_Plan:
    {
		init = clock();
      if( mGoalConf.size() < 0 ){ std::cout << "(x) Must set a goal" << std::endl; break; }
      if( mStartConf.size() < 0 ){ std::cout << "(x) Must set a start" << std::endl; break; }
      if( mWorld == NULL ){ std::cout << "(x) Must load a world" << std::endl; break; }
      if( mWorld->getNumRobots() < 1){ std::cout << "(x) Must load a world with a robot" << std::endl; break; }
      
      double stepSize = 0.02;

      mPlanner = new PathPlanner( *mWorld, false, stepSize );
      //wxThread planThread;
      //planThread.Create();
		if (robotId < 0) { // multi
			int size = 0;
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				size += mWorld->getRobot(i)->getQuickDofs().size();
			}
			mLinks.resize(size);
			int offset = 0;
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				robotics::Robot* robot = mWorld->getRobot(i);
				for (int j = 0; j < robot->getQuickDofsIndices().size(); j++) {
					mLinks(j+offset) = robot->getQuickDofsIndices()(j);
				}
				offset += robot->getQuickDofsIndices().size();
			}
		} else {
			// single
			mLinks.resize(mWorld->getRobot(robotId)->getQuickDofs().size());
			mLinks = mWorld->getRobot(robotId)->getQuickDofsIndices();
		}
      
      bool result = mPlanner->planPath(
					robotId,
					mLinks, 
					mStartConf, 
					mGoalConf, 
					robotSubset,
					mRrtStyle,  
					mConnectMode,
					mGreedyMode,
					mGreedyRatio,
					mSmooth, 
					MAX_NODES);
      if( result  )
	{  SetTimeline(); }
    }

	std::cout << "\n\nTime for HD plan: " << (double)(clock()-init)/((double)CLOCKS_PER_SEC) << std::endl;
    break;
    
    /** Update Time */
  case button_UpdateTime:
    {
      /// Update the time span of the movie timeline
      SetTimeline();
    }		
    break;

  case button_UpdateGreedyRatio:
	  {
		  long T;
		  mGreedyText->GetValue().ToLong(&T);
		  if( T > 10 || T < 1 )
		  {
			  std::cout << "(x) Must be value between 1-10.  Setting to 0 (default). Greedy Ratio = 0%" << std::endl;
			  mGreedyRatio = (int) 0;
			  return;
		  } else {
			  std::cout << "(!) Greedy Ratio = " << T << "0%" << endl;
			  mGreedyRatio = (int) T;
		  }
	  }
	  break;

  case button_UpdateRobotId:
	  {
		  long T;
		  mRobotText->GetValue().ToLong(&T);
		  if( T >= mWorld->getNumRobots() || T < 0 )
		  {
			  robotId = -1;
			  mRobotText->SetValue(wxT("-1"));
			  return;
		  } else {
			  robotId = (int) T;
		  }
	  }
	  break;

    /** Show Path */
  case button_ShowPath:       
    if( mWorld == NULL || mPlanner == NULL || mPlanner->path.size() == 0 ) {
      std::cout << "(!) Must create a valid plan before printing."<<std::endl;
      return;
    } else {
      std::cout<<"(i) Printing...Implement me :)"<<std::endl;
    }        
    break;
  }
}

/**
 * Sets the Dofs for all the robots and checks for collision
 */
bool RipPlannerTab::checkCollision(const Eigen::VectorXd& conf, const Eigen::VectorXi& links) {
	int offset1 = 0;
	int offset2 = 0;
	for (int i = 0; i < mWorld->getNumRobots(); i++) {
		robotics::Robot* robot = mWorld->getRobot(i);

		Eigen::VectorXd pose;
		Eigen::VectorXi link;
		pose.resize(robot->getQuickDofs().size());
		link.resize(robot->getQuickDofsIndices().size());
		for (int j = offset1; j < offset1+pose.size(); j++) {
			pose(j-offset1) = conf(j);
		}
		for (int j = offset2; j < offset2+link.size(); j++) {
			link(j-offset2) = links(j);
		}
		offset1 += pose.size();
		offset2 += link.size();
		robot->setDofs(pose, link);
		robot->update();
	}
	return mWorld->checkCollision();
}


void RipPlannerTab::SetTimeline(std::list<Eigen::VectorXd> path) {

	double T;
    mTimeText->GetValue().ToDouble(&T);

    int numsteps = path.size();
    double increment = T/(double)numsteps;

    cout << "-->(+) Updating Timeline - Increment: " << increment << " Total T: " << T << " Steps: " << numsteps << endl;

    frame->InitTimer( string("RRT_Plan"),increment );

    //Eigen::VectorXd vals( mLinks.size() );

    for( std::list<Eigen::VectorXd>::iterator it = path.begin(); it != path.end(); it++ ) {

		//std::cout << "Rendering frame " << turn << " (" << mPlanner->path.size() << ")\n";

	
			int offset = 0;
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				robotics::Robot* robot = mWorld->getRobot(i);

				Eigen::VectorXd pose;
				pose.resize(robot->getQuickDofs().size());
				for (int j = offset; j < offset+pose.size(); j++) {
					pose(j-offset) = (*it)(j);
				}
				offset += pose.size();
				robot->setQuickDofs(pose);
				robot->update();
			}
		

        frame->AddWorld( mWorld );
    }



}

/**
 * @function setTimeLine
 * @brief 
 */
void RipPlannerTab::SetTimeline() {

    if( mWorld == NULL || mPlanner == NULL || mPlanner->path.size() == 0 ) {
        cout << "--(!) Must create a valid plan before updating its duration (!)--" << endl;
	return;
    }

    double T;
    mTimeText->GetValue().ToDouble(&T);

    int numsteps = mPlanner->path.size();
    double increment = T/(double)numsteps;

    cout << "-->(+) Updating Timeline - Increment: " << increment << " Total T: " << T << " Steps: " << numsteps << endl;

    frame->InitTimer( string("RRT_Plan"),increment );

    Eigen::VectorXd vals( mLinks.size() );

    for( std::list<Eigen::VectorXd>::iterator it = mPlanner->path.begin(); it != mPlanner->path.end(); it++ ) {

		//std::cout << "Rendering frame " << turn << " (" << mPlanner->path.size() << ")\n";

		if (robotId < 0) { // multi
			int offset = 0;
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				robotics::Robot* robot = mWorld->getRobot(i);

				Eigen::VectorXd pose;
				pose.resize(robot->getQuickDofs().size());
				for (int j = offset; j < offset+pose.size(); j++) {
					pose(j-offset) = (*it)(j);
				}
				offset += pose.size();
				robot->setQuickDofs(pose);
				robot->update();
			}
		} 
		
		else { // single
			mWorld->getRobot(robotId)->setQuickDofs(*it);
			mWorld->getRobot(robotId)->update();
		}

        frame->AddWorld( mWorld );
    }

}

/**
 * @function OnCheckBox
 * @brief Handle CheckBox Events
 */ 
void RipPlannerTab::OnCheckBox( wxCommandEvent &evt ) {
  int checkbox_num = evt.GetId();
  
  switch (checkbox_num) {
    
  case checkbox_beGreedy:
    mGreedyMode = (bool)evt.GetSelection();
    std::cout << "(i) greedy = " << mGreedyMode << std::endl;
    break;
    
  case checkbox_useConnect:
    mConnectMode = (bool)evt.GetSelection();
    std::cout << "(i) useConnect = " << mConnectMode << std::endl;
    break;
  case checkbox_useSmooth:
    mSmooth = (bool)evt.GetSelection();
    std::cout << "(i) Smooth option = " << mSmooth << std::endl;
    break;
  }
}

/**
 * @function OnSlider
 * @brief Handle slider changes
 */
void RipPlannerTab::OnSlider(wxCommandEvent &evt) {
  if (selectedTreeNode == NULL) {
    return;
  }
  
  int slnum = evt.GetId();
  double pos = *(double*) evt.GetClientData();
  char numBuf[1000];
  
  switch (slnum) {
  case slider_Time:
    sprintf(numBuf, "X Change: %7.4f", pos);
    std::cout << "(i) Timeline slider output: " << numBuf << std::endl;
    //handleTimeSlider(); // uses slider position to query plan state
    break;
    
  default:
    return;
  }
  //world->updateCollision(o);
  //viewer->UpdateCamera();
  
  if (frame != NULL)
    frame->SetStatusText(wxString(numBuf, wxConvUTF8));
}

/**
 * @function GRIPStateChange
 * @brief This function is called when an object is selected in the Tree View or other
 *        global changes to the RST world. Use this to capture events from outside the tab.
 */
void RipPlannerTab::GRIPStateChange() {
	//init_objs();


	if ( selectedTreeNode == NULL ) {
		return;
	}
  
	std::string statusBuf;
	std::string buf, buf2;
  
	switch (selectedTreeNode->dType) {
    
	case Return_Type_Object:
		mSelectedObject = (robotics::Object*) ( selectedTreeNode->data );
		statusBuf = " Selected Object: " + mSelectedObject->getName();
		buf = "You clicked on object: " + mSelectedObject->getName();
    
		// Enter action for object select events here:
    
		break;
	case Return_Type_Robot:
		mSelectedRobot = (robotics::Robot*) ( selectedTreeNode->data );
		statusBuf = " Selected Robot: " + mSelectedRobot->getName();
		buf = " You clicked on robot: " + mSelectedRobot->getName();
    
		// Enter action for Robot select events here:
    
		break;
	case Return_Type_Node:
		mSelectedNode = (dynamics::BodyNodeDynamics*) ( selectedTreeNode->data );
		statusBuf = " Selected Body Node: " + string(mSelectedNode->getName()) + " of Robot: "
			+ ( (robotics::Robot*) mSelectedNode->getSkel() )->getName();
		buf = " Node: " + std::string(mSelectedNode->getName()) + " of Robot: " + ( (robotics::Robot*) mSelectedNode->getSkel() )->getName();
    
		// Enter action for link select events here:
    
		break;
	default:
		fprintf(stderr, "--( :D ) Someone else's problem!\n");
		assert(0);
		exit(1);
	}
  
	//cout << buf << endl;
	frame->SetStatusText(wxString(statusBuf.c_str(), wxConvUTF8));
	sizerFull->Layout();
}

/**
 * Determines all of the robots that are colliding in each step from [start, end)
 * and populates robotSubset with the results
 *@param start the first step where there is a collision
 *@param end the step after the last collision
 */
void RipPlannerTab::collidingRobots(std::list<Eigen::VectorXd>::iterator start, std::list<Eigen::VectorXd>::iterator end) {
	robotSubset.clear();
	std::vector<bool> collides;
	for (int i = 0; i < mWorld->getNumRobots(); i++) {
		collides.push_back(false);
	}
	for (std::list<Eigen::VectorXd>::iterator i = start; i != end; i++) {
		int offset = 0;
		for (int k = 0; k < mWorld->getNumRobots(); k++) {
			Eigen::VectorXd pose;
			robotics::Robot* robot = mWorld->getRobot(k);
			pose.resize(robot->getQuickDofs().size());
			//if (!collides[k]) {	

				for (int j = offset; j < offset+pose.size(); j++) {
					pose(j-offset) = (*i)(j);
				}
				robot->setQuickDofs(pose);
				robot->update();

				// compare with other robots
				int suboffset = 0;
				for (int l = 0; l < mWorld->getNumRobots(); l++) {
					robotics::Robot* robot2 = mWorld->getRobot(l);
					pose.resize(robot2->getQuickDofs().size());
					if (l != k) {
						for (int j = suboffset; j < suboffset+pose.size(); j++) {
							pose(j-suboffset) = (*i)(j);
						}
						robot2->setQuickDofs(pose);
						robot2->update();
						if (mWorld->checkCollision()) {
							collides[k] = true;
							collides[l] = true;
						}
						// reset pose
						for (int j = suboffset; j < suboffset+pose.size(); j++) {
							pose(j-suboffset) = mStartConf(j);
						}
						robot2->setQuickDofs(pose);
						robot2->update();
					}
					suboffset += pose.size();
				}
				//

				// reset pose
				pose.resize(robot->getQuickDofs().size());
				for (int j = offset; j < offset+pose.size(); j++) {
					pose(j-offset) = mStartConf(j);
				}
				robot->setQuickDofs(pose);
				robot->update();
			//} // at least 2 of these robots are colliding now!
			offset += pose.size();
		}
	}
	for (int i = 0; i < collides.size(); i++) {
		if (collides[i]) {
			robotSubset.push_back(i);
			std::cout << "Subrobot: " << i << "\n";
		}
	}
	/// (!) robotSubset needs to be an ordered list of robot Ids
	///     ie- [0, 2, 3] is OK, but [4, 2] is not!
}
